#include "atomic_0701_ysdd.h"

// 计算两点间距离(大圆距离)
double calculateDistance(double lat1, double lon1, double lat2, double lon2)
{
    double phi1 = lat1 * PI / 180.0;
    double phi2 = lat2 * PI / 180.0;
    double delta_phi = (lat2 - lat1) * PI / 180.0;
    double delta_lambda = (lon2 - lon1) * PI / 180.0;

    double a = sin(delta_phi / 2) * sin(delta_phi / 2) +
               cos(phi1) * cos(phi2) *
                   sin(delta_lambda / 2) * sin(delta_lambda / 2);
    return 2 * EARTH_RADIUS * atan2(sqrt(a), sqrt(1 - a));
}

// 计算初始航向
double calculateBearing(double lat1, double lon1, double lat2, double lon2)
{
    double phi1 = lat1 * PI / 180.0;
    double phi2 = lat2 * PI / 180.0;
    double lambda1 = lon1 * PI / 180.0;
    double lambda2 = lon2 * PI / 180.0;

    double y = sin(lambda2 - lambda1) * cos(phi2);
    double x = cos(phi1) * sin(phi2) - sin(phi1) * cos(phi2) * cos(lambda2 - lambda1);
    return fmod(atan2(y, x) * 180.0 / PI + 360.0, 360.0);
}

// 预设弹道信息主函数
int getPresetTrajectoryInfo(PresetTrajectoryInput *input,
                            PresetTrajectoryOutput *output)
{
    // 0. 初始化随机种子
    srand((unsigned int)(input->timestamp * 1000));

    // 1. 计算总距离和初始航向
    double total_distance = calculateDistance(
        input->current_latitude, input->current_longitude,
        input->target_latitude, input->target_longitude);

    double initial_bearing = calculateBearing(
        input->current_latitude, input->current_longitude,
        input->target_latitude, input->target_longitude);

    // 2. 确定航路点数量 (1-3个)
    output->waypoint_count = 1 + rand() % 3;
    if (total_distance < 50000)
        output->waypoint_count = 1;

    // 3. 生成航路点坐标 (不使用数组)
    if (output->waypoint_count >= 1)
    {
        double ratio1 = 0.33;
        output->waypoint1_lat = input->current_latitude +
                                (input->target_latitude - input->current_latitude) * ratio1;
        output->waypoint1_lon = input->current_longitude +
                                (input->target_longitude - input->current_longitude) * ratio1;
        output->waypoint1_alt = input->current_altitude +
                                (input->target_altitude - input->current_altitude) * ratio1;

        // 添加随机扰动
        output->waypoint1_lat += ((double)rand() / RAND_MAX - 0.5) * 0.1;
        output->waypoint1_lon += ((double)rand() / RAND_MAX - 0.5) * 0.1;
        output->waypoint1_alt += ((double)rand() / RAND_MAX - 0.5) * 100;
    }

    if (output->waypoint_count >= 2)
    {
        double ratio2 = 0.66;
        output->waypoint2_lat = input->current_latitude +
                                (input->target_latitude - input->current_latitude) * ratio2;
        output->waypoint2_lon = input->current_longitude +
                                (input->target_longitude - input->current_longitude) * ratio2;
        output->waypoint2_alt = input->current_altitude +
                                (input->target_altitude - input->current_altitude) * ratio2;

        // 添加随机扰动
        output->waypoint2_lat += ((double)rand() / RAND_MAX - 0.5) * 0.1;
        output->waypoint2_lon += ((double)rand() / RAND_MAX - 0.5) * 0.1;
        output->waypoint2_alt += ((double)rand() / RAND_MAX - 0.5) * 100;
    }

    if (output->waypoint_count >= 3)
    {
        double ratio3 = 0.5;
        output->waypoint3_lat = input->current_latitude +
                                (input->target_latitude - input->current_latitude) * ratio3;
        output->waypoint3_lon = input->current_longitude +
                                (input->target_longitude - input->current_longitude) * ratio3;
        output->waypoint3_alt = input->current_altitude +
                                (input->target_altitude - input->current_altitude) * ratio3;

        // 添加随机扰动
        output->waypoint3_lat += ((double)rand() / RAND_MAX - 0.5) * 0.1;
        output->waypoint3_lon += ((double)rand() / RAND_MAX - 0.5) * 0.1;
        output->waypoint3_alt += ((double)rand() / RAND_MAX - 0.5) * 100;
    }

    // 4. 计算推荐速度 (考虑最大速度限制)
    output->target_velocity = fmin(
        input->max_velocity,
        input->current_velocity + total_distance * 0.0005);

    // 5. 计算推荐航向 (考虑当前航向平滑过渡)
    output->target_angle = initial_bearing;
    if (fabs(initial_bearing - input->current_heading) > 180.0)
    {
        if (initial_bearing > input->current_heading)
        {
            output->target_angle -= 360.0;
        }
        else
        {
            output->target_angle += 360.0;
        }
    }
    output->target_angle =
        input->current_heading * 0.3 + output->target_angle * 0.7;
    output->target_angle = fmod(output->target_angle + 360.0, 360.0);

    // 6. 计算爬升角 (基于高度变化)
    double total_climb = input->target_altitude - input->current_altitude;
    output->climb_angle = atan(total_climb / total_distance) * 180.0 / PI;

    // 7. 计算到目标距离
    output->target_altitude = total_distance;

    // 8. 计算预计时间 (分段计算)
    output->estimated_time = 0.0;
    if (output->waypoint_count >= 1)
    {
        double seg1_dist = calculateDistance(
            input->current_latitude, input->current_longitude,
            output->waypoint1_lat, output->waypoint1_lon);
        output->estimated_time += seg1_dist / ((input->current_velocity + output->target_velocity) / 2);
    }
    if (output->waypoint_count >= 2)
    {
        double seg2_dist = calculateDistance(
            output->waypoint1_lat, output->waypoint1_lon,
            output->waypoint2_lat, output->waypoint2_lon);
        output->estimated_time += seg2_dist / output->target_velocity;
    }
    if (output->waypoint_count >= 3)
    {
        double seg3_dist = calculateDistance(
            output->waypoint2_lat, output->waypoint2_lon,
            output->waypoint3_lat, output->waypoint3_lon);
        output->estimated_time += seg3_dist / output->target_velocity;
    }
    double final_seg_dist = calculateDistance(
        (output->waypoint_count == 1) ? output->waypoint1_lat : (output->waypoint_count == 2) ? output->waypoint2_lat
                                                                                              : output->waypoint3_lat,
        (output->waypoint_count == 1) ? output->waypoint1_lon : (output->waypoint_count == 2) ? output->waypoint2_lon
                                                                                              : output->waypoint3_lon,
        input->target_latitude, input->target_longitude);
    output->estimated_time += final_seg_dist / output->target_velocity;

    // 9. 计算最大允许偏差 (基于距离的百分比)
    output->max_deviation = total_distance * 0.01;
    if (output->max_deviation < 50.0)
        output->max_deviation = 50.0;
    if (output->max_deviation > 500.0)
        output->max_deviation = 500.0;

    // 10. 计算能量消耗 (简化模型)
    output->energy_consumption = total_distance *
                                 output->target_velocity * output->target_velocity * 0.0001;

    // 11. 计算风险系数 (基于高度变化和速度)
    output->risk_factor = 0.0;
    if (output->waypoint_count >= 1)
    {
        double alt_change1 = fabs(output->waypoint1_alt - input->current_altitude);
        output->risk_factor += alt_change1 * 0.0002;
    }
    if (output->waypoint_count >= 2)
    {
        double alt_change2 = fabs(output->waypoint2_alt - output->waypoint1_alt);
        output->risk_factor += alt_change2 * 0.0002;
    }
    if (output->waypoint_count >= 3)
    {
        double alt_change3 = fabs(output->waypoint3_alt - output->waypoint2_alt);
        output->risk_factor += alt_change3 * 0.0002;
    }
    output->risk_factor += (output->target_velocity / input->max_velocity) * 0.3;
    output->risk_factor = fmin(output->risk_factor, 1.0);

    // 12. 计算轨迹评分 (0-100)
    output->trajectory_score = 100.0 - output->risk_factor * 40.0 -
                               (output->estimated_time / 3600.0) * 10.0;
    output->trajectory_score = fmax(fmin(output->trajectory_score, 100.0), 0.0);

    // 13. 环境补偿计算
    output->wind_compensation = 0.0;
    output->temp_compensation = 0.0;

    // 14. 添加复杂的环境适应计算 (扩充行数)
    for (int i = 0; i < 50; i++)
    {
        // 模拟风场影响
        double wind_speed = 10.0 + 5.0 * sin(input->timestamp * 0.01 + i * 0.1);
        double wind_dir = fmod(180.0 + 30.0 * sin(input->timestamp * 0.005), 360.0);

        // 风补偿计算
        double wind_angle = fmod(wind_dir - output->target_angle + 360.0, 360.0);
        double headwind = wind_speed * cos(wind_angle * PI / 180.0);
        output->wind_compensation += headwind * 0.01;

        // 温度补偿计算
        double temp = 15.0 - 0.0065 * input->current_altitude +
                      5.0 * sin(input->timestamp * 0.001 + i * 0.02);
        output->temp_compensation += (temp - 15.0) * 0.001;
    }
    output->wind_compensation /= 50.0;
    output->temp_compensation /= 50.0;

    return 0;
}